#include "sdkeli_ls_common_udp.h"
#include "sdkeli_ls1207de_parser.h"

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("lskeli_udp_node",rclcpp::NodeOptions(rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)));

    /*Check whether hostname is provided*/
    bool isTcpConnection = false;
    std::string strHostName;
    std::string strPort;

    node->declare_parameter<std::string>("port","2112");
    node->declare_parameter<int>("timelimit",5);
    node->declare_parameter<bool>("subscribe_datagram",false);
    node->declare_parameter<int>("device_number",0);

  
    if(node->get_parameter<std::string>("hostname",strHostName))
    {
        isTcpConnection = true;
        node->get_parameter<std::string>("port",strPort);
    
    }

    /*Get configured time limit*/
    int iTimeLimit = 5;
    node->get_parameter<int>("timelimit",iTimeLimit);

    bool isDataSubscribed = false;
    node->get_parameter<bool>("subscribe_datagram",isDataSubscribed);

    int iDeviceNumber = 0;
    node->get_parameter<int>("device_number",iDeviceNumber);

    /*Create and initialize parser*/
    sdkeli_ls_udp::CSDKeliLs1207DEParser *pParser = new sdkeli_ls_udp::CSDKeliLs1207DEParser(node);

    double param;
    std::string frame_id;

    if(node->get_parameter<double>("range_min",param))
    {
        RCLCPP_INFO(node->get_logger(),"range_min: %f", param);
        pParser->SetRangeMin(param);
    }

    if(node->get_parameter<double>("range_max",param))
    {
        RCLCPP_INFO(node->get_logger(),"range_max: %f", param);
        pParser->SetRangeMax(param);
    }

    if(node->get_parameter<double>("time_increment",param))
    {
        RCLCPP_INFO(node->get_logger(),"time_increment: %f", param);
        pParser->SetTimeIncrement(param);
    }

    if(node->get_parameter<std::string>("frame_id",frame_id))
    {
        RCLCPP_INFO(node->get_logger(),"frame_id: %s", frame_id.c_str());
        pParser->SetFrameId(frame_id);
    }

    /*Setup UDP connection and attempt to connect/reconnect*/
    sdkeli_ls_udp::CSDKeliLsCommon *pSDKeliLs = NULL;
    int result = sdkeli_ls_udp::ExitError;
    while(rclcpp::ok())
    {
        if(pSDKeliLs != NULL)
        {
            delete pSDKeliLs;
        }

        pSDKeliLs = new sdkeli_ls_udp::CSDKeliLsCommonUdp(strHostName, strPort, iTimeLimit, pParser,node);
        result = pSDKeliLs->Init();

        /*Device has been initliazed successfully*/
        while(rclcpp::ok() && (result == sdkeli_ls_udp::ExitSuccess))
        {            
            rclcpp::spin_some(node);
            result = pSDKeliLs->LoopOnce();
        }

        if(result == sdkeli_ls_udp::ExitFatal)
        {
            return result;
        }
    }

    if(pSDKeliLs != NULL)
    {
        delete pSDKeliLs;
    }

    if(pParser != NULL)
    {
        delete pParser;
    }

    return result;
}
